#include "flamingdeath_hardware.h"
#include "flamingdeath.h"
#include "stm32f10x_rcc.h"
#include "stm32f10x_gpio.h"
#include "stm32f10x_spi.h"
#include "stm32f10x_i2c.h"
#include "stm32f10x_tim.h"
#include "stm32f10x_usart.h"
#include "misc.h" /* wtf is NVIC stuff doing in misc.h? */
#include "delay.h"
//#include "navigation.h"
#include "gps_parse.h"
#include "buffer64.h"


/*============================ LED Config ===================================*/

/* initialize the LED */
static void flamingdeath_LEDInit()
{
  GPIO_InitTypeDef GPIO_InitStructure;

  /* enable the clock for the LED */
  RCC_APB2PeriphClockCmd(LED_GPIO_CLK, ENABLE);

  /* set up the GPIO */
  GPIO_InitStructure.GPIO_Pin = LED_PIN;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  /* initialize the GPIO */
  GPIO_Init(LED_GPIO_PORT, &GPIO_InitStructure);
}

/* turn on the LED */
void flamingdeath_LEDOn()
{
  LED_GPIO_PORT->BRR = LED_PIN;
}

/* turn off the LED */
void flamingdeath_LEDOff()
{
  LED_GPIO_PORT->BSRR = LED_PIN;
}

/* toggle the LED */
void flamingdeath_LEDToggle()
{
  LED_GPIO_PORT->ODR ^= LED_PIN;
}

/*========================= Button Config ===================================*/

/* initialize the onboard push button */
static void flamingdeath_ButtonInit()
{
  GPIO_InitTypeDef GPIO_InitStructure;
  /* Enable the BUTTON Clock */
  RCC_APB2PeriphClockCmd(BUT_GPIO_CLK, ENABLE);

  /* Configure Button pin as input floating */
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
  GPIO_InitStructure.GPIO_Pin = BUT_PIN;
  GPIO_Init(BUT_GPIO_PORT, &GPIO_InitStructure);
}

/* read the value of the button */
uint32_t flamingdeath_ButtonRead()
{
  return !GPIO_ReadInputDataBit(BUT_GPIO_PORT, BUT_PIN);
}

/*===================== Interrupts Config ===================================*/

static void NVIC_Configuration()
{
  NVIC_InitTypeDef NVIC_InitStructure;

  /* configure preemption priority bits */
  NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0);

  /* enable the bluetooth interrupt */
  NVIC_InitStructure.NVIC_IRQChannel = BT_IRQn;
  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
  NVIC_Init(&NVIC_InitStructure);

  /* enable the GPS interrupt */
  NVIC_InitStructure.NVIC_IRQChannel = GPS_IRQn;
  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
  NVIC_Init(&NVIC_InitStructure);

  /* enable the accelerometer interrupt */
  NVIC_InitStructure.NVIC_IRQChannel = ACCEL_IRQn;
  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
  NVIC_Init(&NVIC_InitStructure);

#if 0
  /* enable the accelerometer interrupt */
  NVIC_InitStructure.NVIC_IRQChannel = COMP_IRQn;
  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
  NVIC_Init(&NVIC_InitStructure);
#endif

  /* enable the accelerometer interrupt */
  NVIC_InitStructure.NVIC_IRQChannel = SENSOR_TIME_IRQn;
  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
  NVIC_Init(&NVIC_InitStructure);
}

/*======================== Bluetooth Config =================================*/

static buffer64 bt_send;
static buffer64 bt_receive;

/* bluetooth interrupt handler */
void USART1_IRQHandler()
{
  /* handle a receive interrupt */
  if(USART_GetITStatus(BT_DEVICE, USART_IT_RXNE) != RESET) {
    /* blindly write data to the circular buffer. */
    buffer64_insert(&bt_receive, USART_ReceiveData(BT_DEVICE));
  }

  /* handle a transmit interrupt */
  if(USART_GetITStatus(BT_DEVICE, USART_IT_TXE) != RESET) {
    /* read one byte from the bt circular buffer */
    USART_SendData(BT_DEVICE, buffer64_remove(&bt_send));

    /* if there are no more bytes to send, disable the transmit interrupt */
    if(buffer64_empty(&bt_send))
      USART_ITConfig(BT_DEVICE, USART_IT_TXE, DISABLE);

  }
}

/* enable and configure USART1 for bluetooth */
static void flamingdeath_BTInit()
{
  USART_InitTypeDef USART_InitStructure;
  GPIO_InitTypeDef GPIO_InitStructure;

  /* initialize the transmit and receive buffers */
  buffer64_init(&bt_send);
  buffer64_init(&bt_receive);

  /* enable the clocks */
  RCC_APB2PeriphClockCmd(BT_CLK, ENABLE);
  RCC_APB2PeriphClockCmd(BT_RX_GPIO_CLK, ENABLE);
  RCC_APB2PeriphClockCmd(BT_TX_GPIO_CLK, ENABLE);

  /* configure the USART */
  USART_InitStructure.USART_BaudRate = BT_BAUDRATE;
  USART_InitStructure.USART_WordLength = USART_WordLength_8b;
  USART_InitStructure.USART_StopBits = USART_StopBits_1;
  USART_InitStructure.USART_Parity = USART_Parity_No;
  USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
  USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
  USART_Init(BT_DEVICE, &USART_InitStructure);

  /* configure the pins */

  /* Rx pin is floating input */
  GPIO_InitStructure.GPIO_Pin = BT_RX_PIN;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
  GPIO_Init(BT_RX_GPIO_PORT, &GPIO_InitStructure);

  /* Tx pin is Alternate Push Pull */
  GPIO_InitStructure.GPIO_Pin = BT_TX_PIN;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
  GPIO_Init(BT_TX_GPIO_PORT, &GPIO_InitStructure);

  /* enable receive interrupt */
  USART_ITConfig(BT_DEVICE, USART_IT_RXNE, ENABLE);

  /* enable the usart */
  USART_Cmd(BT_DEVICE, ENABLE);
}

void flamingdeath_BTSend(char c)
{
  while(buffer64_full(&bt_send));
  buffer64_insert(&bt_send, c);
  USART_ITConfig(BT_DEVICE, USART_IT_TXE, ENABLE);
}

char flamingdeath_BTRecv()
{
  while(buffer64_empty(&bt_receive));
  return buffer64_remove(&bt_receive);
}

int flamingdeath_BTRecvReady()
{ return !buffer64_empty(&bt_receive); }

int flamingdeath_BTSendComplete()
{ return buffer64_empty(&bt_send); }

int flamingdeath_BTSendReady()
{ return !buffer64_full(&bt_send); }

void flamingdeath_BTFlush()
{ while(!buffer64_empty(&bt_send)); }

/*=============================== GPS Config ================================*/

gps_t gps;
static buffer64 gps_send;

/* gps interrupt handler */
void USART2_IRQHandler()
{
  /* handle a receive interrupt */
  if(USART_GetITStatus(GPS_DEVICE, USART_IT_RXNE) != RESET) {
    char c = USART_ReceiveData(GPS_DEVICE);

    //flamingdeath_BTSend(c);
    gps_encode(c, &gps);
  }

  /* handle a transmit interrupt */
  if(USART_GetITStatus(GPS_DEVICE, USART_IT_TXE) != RESET) {
    /* read one byte from the bt circular buffer */
    USART_SendData(GPS_DEVICE, buffer64_remove(&gps_send));

    /* if there are no more bytes to send, disable the transmit interrupt */
    if(buffer64_empty(&gps_send))
      USART_ITConfig(GPS_DEVICE, USART_IT_TXE, DISABLE);

  }
}

/* enable and configure USART2 for gps */
static void flamingdeath_GPSInit()
{
  USART_InitTypeDef USART_InitStructure;
  GPIO_InitTypeDef GPIO_InitStructure;

  gps_init(&gps);

  /* initialize the transmit and receive buffers */
  buffer64_init(&gps_send);

  /* enable the clocks */
  RCC_APB1PeriphClockCmd(GPS_CLK, ENABLE);
  RCC_APB2PeriphClockCmd(GPS_RX_GPIO_CLK, ENABLE);
  RCC_APB2PeriphClockCmd(GPS_TX_GPIO_CLK, ENABLE);

  /* configure the USART */
  USART_InitStructure.USART_BaudRate = GPS_BAUDRATE;
  USART_InitStructure.USART_WordLength = USART_WordLength_8b;
  USART_InitStructure.USART_StopBits = USART_StopBits_1;
  USART_InitStructure.USART_Parity = USART_Parity_No;
  USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
  USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
  USART_Init(GPS_DEVICE, &USART_InitStructure);

  /* configure the pins */

  /* Rx pin is floating input */
  GPIO_InitStructure.GPIO_Pin = GPS_RX_PIN;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
  GPIO_Init(GPS_RX_GPIO_PORT, &GPIO_InitStructure);

  /* Tx pin is Alternate Push Pull */
  GPIO_InitStructure.GPIO_Pin = GPS_TX_PIN;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
  GPIO_Init(GPS_TX_GPIO_PORT, &GPIO_InitStructure);

  /* enable receive interrupt */
  USART_ITConfig(GPS_DEVICE, USART_IT_RXNE, ENABLE);

  /* enable the usart */
  USART_Cmd(GPS_DEVICE, ENABLE);
}

void flamingdeath_GPSSend(char c)
{
  while(buffer64_full(&gps_send));
  buffer64_insert(&gps_send, c);
  USART_ITConfig(GPS_DEVICE, USART_IT_TXE, ENABLE);
}

int flamingdeath_GPSSendReady()
{ return !buffer64_full(&gps_send); }

void flamingdeath_GPSFlush()
{ while(!buffer64_empty(&gps_send)); }


uint8_t GPS_send_binary(uint16_t GPS_PL, char *GPS_message) {
  int i;
	// GPS_PL is length of id + message
	// GPS_message most be of length GPS_PL + 1
	
	// GPS_message (checksum added here)
	// 0:			id
	// 1-GPS_PL:	message
	// GPS_PL+1:  checksum
	
	uint8_t chksum = 0;
	for (i = 0; i < GPS_PL; ++i)
		chksum ^= GPS_message[i];
	
	GPS_message[GPS_PL] = chksum;
	
	
	// start
	flamingdeath_GPSSend(0xA0);
	flamingdeath_GPSSend(0xA1);
	
	// length
	flamingdeath_GPSSend(GPS_PL>>8);
	flamingdeath_GPSSend(GPS_PL&0x0000FFFF);
	
	// message
	for (i = 0; i <= GPS_PL; ++i)
		flamingdeath_GPSSend(GPS_message[i]);
	
	// end
	flamingdeath_GPSSend(0x0D);
	flamingdeath_GPSSend(0x0A);
	
	flamingdeath_GPSFlush();
	
	return 1;
}

uint8_t GPS_sent_init_messages() {
	// 0:			id
	// 1-GPS_PL:	message
	uint16_t GPS_PL;
	char GPS_message[20];
  int i;
#if 0

  for(i = 0; i < 50; i++) {
    GPS_PL = 2;
    GPS_message[0] = 0x04;
    GPS_message[1] = 0x01;
    GPS_send_binary(GPS_PL,GPS_message);
  }
#endif
	
	// messages
  for(i = 0; i < 10; i++) {
    GPS_PL = 9;
    GPS_message[0] = 0x08;
    GPS_message[1] = 0x01;	// GGA
    GPS_message[2] = 0x00;	// GSA
    GPS_message[3] = 0x00;	// GSV
    GPS_message[4] = 0x00;	// GLL
    GPS_message[5] = 0x00;	// RMC
    GPS_message[6] = 0x00;	// VTG
    GPS_message[7] = 0x00;	// ZDA
    GPS_message[8] = 0x01;	// attributed
    
    GPS_send_binary(GPS_PL,GPS_message);
  }
	
#if 0
	// serial frequency
	GPS_PL = 4;
	GPS_message[0] = 0x05;
	GPS_message[1] = 0x00;	// com 1
	GPS_message[2] = 0x01;	// 38400
	GPS_message[3] = 0x01;	// attributes

  GPS_send_binary(GPS_PL,GPS_message);
#endif
	
	GPS_send_binary(GPS_PL,GPS_message);
	
	// gps frequency
  for(i = 0; i < 10; i++) {
    GPS_PL = 3;
    GPS_message[0] = 0x0E;
    GPS_message[1] = 0x0A;	// 1-10 (>2 requires higher BAUD rate 38400 or higher)
    GPS_message[2] = 0x01;	// attributes

    GPS_send_binary(GPS_PL,GPS_message);
  }
	
	
	// enable WAAS
  for(i = 0; i < 10; i++) {
    GPS_PL = 3;
    GPS_message[0] = 0x37;
    GPS_message[1] = 0x01;	// enable
    GPS_message[2] = 0x01;	// attributes
    
    GPS_send_binary(GPS_PL,GPS_message);
  }
	
	
	// navigation mode : {car 00, pedestrian 01} 
  for(i = 0; i < 10; i++) {
    GPS_PL = 3;
    GPS_message[0] = 0x3c;
    GPS_message[1] = 0x00;	// car/pedestrian
    GPS_message[2] = 0x01;	// attributes
    GPS_send_binary(GPS_PL,GPS_message);
  }
	
	return 1;
}
/*============================= Accelerometer Init ==========================*/

#define Accel_CS_HIGH() GPIO_SetBits(ACCEL_CS_GPIO_PORT, ACCEL_CS_PIN)
#define Accel_CS_LOW() GPIO_ResetBits(ACCEL_CS_GPIO_PORT, ACCEL_CS_PIN)

static void spi_delay()
{
  uint32_t us=11;
  asm volatile("   mov r0, %[us] \n\t"
               "1: subs r0, #1   \n\t"
               "   bhi 1b        \n\t"
               :
               : [us] "r" (us)
               : "r0");

}

/* contains the axis that it is currently reading */
static uint8_t accel_state;

/* used to build up the acceleration data incrementally */
static uint16_t accel_x_raw_tmp;
static uint16_t accel_y_raw_tmp;
static uint16_t accel_z_raw_tmp;

/* result variables for acceleration data */
volatile int16_t accel_x_raw;
volatile int16_t accel_y_raw;
volatile int16_t accel_z_raw;
/* incremented after a write to the acceleration data */
volatile uint32_t accel_read_id;

static void flamingdeath_AccelInit()
{
  GPIO_InitTypeDef GPIO_InitStructure;
  SPI_InitTypeDef SPI_InitStructure;

  /* enable clocks */
  RCC_APB2PeriphClockCmd(ACCEL_CLK | ACCEL_SCK_GPIO_CLK | ACCEL_MISO_GPIO_CLK
      | ACCEL_MOSI_GPIO_CLK | ACCEL_CS_GPIO_CLK, ENABLE);

  /* configure outputs */
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;

  /* configure SCK */
  GPIO_InitStructure.GPIO_Pin = ACCEL_SCK_PIN;
  GPIO_Init(ACCEL_SCK_GPIO_PORT, &GPIO_InitStructure);

  /* configure MOSI */
  GPIO_InitStructure.GPIO_Pin = ACCEL_MOSI_PIN;
  GPIO_Init(ACCEL_MOSI_GPIO_PORT, &GPIO_InitStructure);

  /* configure MISO */
  GPIO_InitStructure.GPIO_Pin = ACCEL_MISO_PIN;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
  GPIO_Init(ACCEL_MISO_GPIO_PORT, &GPIO_InitStructure);

  /* configure CS */
  GPIO_InitStructure.GPIO_Pin = ACCEL_CS_PIN;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
  GPIO_Init(ACCEL_CS_GPIO_PORT, &GPIO_InitStructure);

  /* set chip select high */
  Accel_CS_HIGH();

  /* configure spi port */
  SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
  SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
  SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
  SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low;
  SPI_InitStructure.SPI_CPHA = SPI_CPHA_1Edge;
  SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
  SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_32;
  SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
  SPI_InitStructure.SPI_CRCPolynomial = 7;
  SPI_Init(ACCEL_DEVICE, &SPI_InitStructure);

  /* enable spi port */
  SPI_Cmd(ACCEL_DEVICE, ENABLE);

  /* send read command for z msb */
  Accel_CS_LOW();
  spi_delay();
  SPI_I2S_SendData(ACCEL_DEVICE, 9 << 2);
  /* wait until data is transfered */
  while(SPI_I2S_GetFlagStatus(ACCEL_DEVICE, SPI_I2S_FLAG_TXE) == RESET);
  /* wait until a byte is received */
  while(SPI_I2S_GetFlagStatus(ACCEL_DEVICE, SPI_I2S_FLAG_RXNE) == RESET);
  /* read and throw away the data */
  SPI_I2S_ReceiveData(ACCEL_DEVICE);

  /* now ready to receive, in order:
   * z msb
   * z lsb
   * y msb
   * y lsb
   * x msb
   * x lsb
   * ...
   */

  /* set the state machine initial state */
  accel_state = 0;

  /* turn on receive interrupt for SPI */
  SPI_I2S_ITConfig(ACCEL_DEVICE, SPI_I2S_IT_RXNE, ENABLE);
  /* send a byte to start the receive process */
  SPI_I2S_SendData(ACCEL_DEVICE, 0);
}


void SPI1_IRQHandler()
{
  uint16_t dat = SPI_I2S_ReceiveData(ACCEL_DEVICE);
  dat &= 0xFF;
  switch(accel_state) {
    case 0: /* z msb */
      accel_z_raw_tmp = dat << 8;
      accel_state++;
      break;
    case 1: /* z lsb */
      accel_z_raw_tmp |= dat;
      accel_state++;
      break;
    case 2: /* y msb */
      accel_y_raw_tmp = dat << 8;
      accel_state++;
      break;
    case 3: /* y lsb */
      accel_y_raw_tmp |= dat;
      accel_state++;
      break;
    case 4: /* x msb */
      accel_x_raw_tmp = dat << 8;
      accel_state++;
      break;
    case 5: /* x lsb */
      accel_x_raw_tmp |= dat;

      /* copy data to actual fields */
      accel_x_raw = (int16_t)accel_x_raw_tmp;
      accel_y_raw = (int16_t)accel_y_raw_tmp;
      accel_z_raw = (int16_t)accel_z_raw_tmp;
      /* shift to the correct bit positions */
      accel_x_raw >>= 3;
      accel_y_raw >>= 3;
      accel_z_raw >>= 3;
      accel_read_id++;
      
      accel_state = 0;
      break;
  }

  if(accel_state != 5) {
    /* queue up the next read */
    SPI_I2S_SendData(ACCEL_DEVICE, 0);
  }
}

void flamingdeath_AccelStart()
{
  SPI_I2S_SendData(ACCEL_DEVICE, 0);
}


/*============================ Sensor Timer =================================*/

/* 500 times per second update the accelerometer data */
/* GPS data is continuously updated */
void TIM2_IRQHandler()
{
  /* if it's not an update interrupt, don't do anything */
  if(TIM_GetITStatus(SENSOR_TIME_DEVICE, TIM_IT_Update) == RESET)
    return;

  /* do the magic */
  flamingdeath_LEDToggle();

  /* start a capture of accelerometer data */
  flamingdeath_AccelStart();

  /* clear the interrupt */
  TIM_ClearITPendingBit(SENSOR_TIME_DEVICE, TIM_IT_Update);

}

static void flamingdeath_SensorTimerInit()
{
  TIM_TimeBaseInitTypeDef TimeBaseInit;

  /* turn on the clock */
  RCC_APB1PeriphClockCmd(SENSOR_TIME_CLK, ENABLE);


  /* 500 times per second * 100 * 1440 = 72000000 */
  TimeBaseInit.TIM_Prescaler = 100;
  TimeBaseInit.TIM_Period = 1440;
  
  /* count up */
  TimeBaseInit.TIM_CounterMode = TIM_CounterMode_Up;
  /* divide by 1, which somehow means multiply */
  TimeBaseInit.TIM_ClockDivision = TIM_CKD_DIV1;
  /* ignored because only used in advanced timer */
  TimeBaseInit.TIM_RepetitionCounter = 0;
  /* init timer */
  TIM_TimeBaseInit(SENSOR_TIME_DEVICE, &TimeBaseInit);


  /* set up the interrupts */
  TIM_ITConfig(SENSOR_TIME_DEVICE, TIM_IT_Update, ENABLE);

  /* start the timer */
  TIM_Cmd(SENSOR_TIME_DEVICE, ENABLE);
}

/*=============================== PWM Timer =================================*/

static void flamingdeath_ServoTimerInit()
{
  TIM_TimeBaseInitTypeDef TimeBaseInit;
  TIM_OCInitTypeDef OCInit;
  GPIO_InitTypeDef GPIO_InitStructure;

  /* turn on the clock */
  RCC_APB1PeriphClockCmd(SERVO_TIME_CLK, ENABLE);

  /* turn on clock for gpio */
  RCC_APB2PeriphClockCmd(SERVO_STEER_CLK | SERVO_THROTTLE_CLK, ENABLE);

  /* configure GPIO */
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
  GPIO_InitStructure.GPIO_Pin = SERVO_STEER_PIN;
  GPIO_Init(SERVO_STEER_GPIO_PORT, &GPIO_InitStructure);
  GPIO_InitStructure.GPIO_Pin = SERVO_THROTTLE_PIN;
  GPIO_Init(SERVO_THROTTLE_GPIO_PORT, &GPIO_InitStructure);

  /* timer expires ~3.2 million times per second */
  TimeBaseInit.TIM_Prescaler = 22;
  /* count up */
  TimeBaseInit.TIM_CounterMode = TIM_CounterMode_Up;
  /* set count limit to overflow every ~20ms */
  TimeBaseInit.TIM_Period = 65455;
  /* divide by 1, which somehow means multiply */
  TimeBaseInit.TIM_ClockDivision = TIM_CKD_DIV1;
  /* ignored because only used in advanced timer */
  TimeBaseInit.TIM_RepetitionCounter = 0;
  /* init timer */
  TIM_TimeBaseInit(SERVO_TIME_DEVICE, &TimeBaseInit);

  /* set up pwm on channel 3: steering */

  /* use PWM mode 1, which is 1 until timer passes TIM_Pulse value */
  OCInit.TIM_OCMode = TIM_OCMode_PWM1;
  /* Capture compare 3 to output mode */
  OCInit.TIM_OutputState = TIM_OutputState_Enable;
  /* initial high time is 1ms */
  OCInit.TIM_Pulse = 3273;
  /* do something.  I have no idea what this does */
  OCInit.TIM_OCPolarity = TIM_OCPolarity_High;

  TIM_OC3Init(SERVO_TIME_DEVICE, &OCInit);

  /* set up pwm on channel 4: throttle */

  /* use PWM mode 1, which is 1 until timer passes TIM_Pulse value */
  OCInit.TIM_OCMode = TIM_OCMode_PWM1;
  /* Capture compare 3 to output mode */
  OCInit.TIM_OutputState = TIM_OutputState_Enable;
  /* initial high time is 1ms */
  OCInit.TIM_Pulse = 3273;
  /* do something.  I have no idea what this does */
  OCInit.TIM_OCPolarity = TIM_OCPolarity_High;

  TIM_OC4Init(SERVO_TIME_DEVICE, &OCInit);

  /* set up the interrupts */
  /* TIM_ITConfig(SERVO_TIME_DEVICE, TIM_IT_Update, ENABLE); */

  /* start the timer */
  TIM_Cmd(SERVO_TIME_DEVICE, ENABLE);

  /* set initial steering */
  flamingdeath_Steering(0);

  /* set initial throttle */
  flamingdeath_Throttle(-1000);
}

#if 1
void flamingdeath_Steering(int16_t value)
{
  int32_t tmp = value;
  tmp += 32768;

  uint16_t v = 3273;
  v += tmp / 20;
  TIM_SetCompare3(SERVO_TIME_DEVICE, v);
}

void flamingdeath_Throttle(int16_t value)
{
  int32_t tmp = value;
  tmp += 32768;

  uint16_t v = 3273;
  v += tmp / 20;
  TIM_SetCompare4(SERVO_TIME_DEVICE, v);
}
#endif

#if 0
/*=========================== Encoder Timer =================================*/

static int charging;
static uint16_t charge_time;
static uint16_t read_time;
int last_value;
volatile uint32_t ticks;

void TIM4_IRQHandler()
{
  GPIO_InitTypeDef GPIO_InitStructure;
  /* if it's not an update interrupt, don't do anything */
  if(TIM_GetITStatus(ENCODER_TIME_DEVICE, TIM_IT_Update) == RESET)
    return;

  /* clear the interrupt */
  TIM_ClearITPendingBit(ENCODER_TIME_DEVICE, TIM_IT_Update);

  if(charging) {
    charging = 0;
    /* charging is over */
    TIM_SetAutoreload(ENCODER_TIME_DEVICE, read_time);

    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
    GPIO_InitStructure.GPIO_Pin = ENCODER_DATA_PIN;
    GPIO_Init(ENCODER_DATA_GPIO_PORT, &GPIO_InitStructure);
    ENCODER_DATA_GPIO_PORT->BSRR = ENCODER_DATA_PIN;

    /* read input pin */
  } else {
    int value = GPIO_ReadInputDataBit(ENCODER_DATA_GPIO_PORT, ENCODER_DATA_PIN);
    if(value != last_value) {
      ticks++;
    }
    last_value = value;
    charging = 1;
    /* reading is over */
    TIM_SetAutoreload(ENCODER_TIME_DEVICE, charge_time);

    /* turn on charging */
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Pin = ENCODER_DATA_PIN;
    GPIO_Init(ENCODER_DATA_GPIO_PORT, &GPIO_InitStructure);
    ENCODER_DATA_GPIO_PORT->BSRR = ENCODER_DATA_PIN;
    
  }

  /* turn on one pulse mode */
  TIM_SelectOnePulseMode(ENCODER_TIME_DEVICE, TIM_OPMode_Single);

  /* start the timer */
  TIM_Cmd(ENCODER_TIME_DEVICE, ENABLE);
}

static void flamingdeath_EncoderTimerInit()
{
  TIM_TimeBaseInitTypeDef TimeBaseInit;
  GPIO_InitTypeDef GPIO_InitStructure;

  /* turn on the clock */
  RCC_APB1PeriphClockCmd(ENCODER_TIME_CLK, ENABLE);
  RCC_APB2PeriphClockCmd(ENCODER_DATA_CLK, ENABLE);

  /* configure GPIO */
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
  GPIO_InitStructure.GPIO_Pin = ENCODER_DATA_PIN;
  GPIO_Init(ENCODER_DATA_GPIO_PORT, &GPIO_InitStructure);

  charge_time = 30000;
  read_time = 1;
  charging = 1;
  /* start charge */
  ENCODER_DATA_GPIO_PORT->BSRR = ENCODER_DATA_PIN;


  /* 20000 times per second * 100 * 30 = 72000000 */
  TimeBaseInit.TIM_Prescaler = 10;
  TimeBaseInit.TIM_Period = charge_time;
  
  /* count up */
  TimeBaseInit.TIM_CounterMode = TIM_CounterMode_Up;
  /* divide by 1, which somehow means multiply */
  TimeBaseInit.TIM_ClockDivision = TIM_CKD_DIV1;
  /* ignored because only used in advanced timer */
  TimeBaseInit.TIM_RepetitionCounter = 0;
  /* init timer */
  TIM_TimeBaseInit(ENCODER_TIME_DEVICE, &TimeBaseInit);

  /* set up the interrupts */
  TIM_ITConfig(ENCODER_TIME_DEVICE, TIM_IT_Update, ENABLE);

  /* set one pulse mode */
  TIM_SelectOnePulseMode(ENCODER_TIME_DEVICE, TIM_OPMode_Single);

  /* start the timer */
  TIM_Cmd(ENCODER_TIME_DEVICE, ENABLE);
}
#endif


/*================================== Hardware Init ==========================*/

static void print_int_i(int i)
{
  int q = i / 10;
  int r = i % 10;

  if(q)
    print_int(q);

  flamingdeath_BTSend(r + '0');
  while(!flamingdeath_BTSendComplete());
}

void print_int(int i)
{
  if(i < 0) {
    flamingdeath_BTSend('-');
    i = -i;
  }
  print_int_i(i);
}

void print_str(const char* str)
{
  while(*str)
    flamingdeath_BTSend(*str++);
  while(!flamingdeath_BTSendComplete());
}

void flamingdeath_Init()
{
  delay_ms(100);
  flamingdeath_LEDInit();
  flamingdeath_ButtonInit();
  NVIC_Configuration();
  flamingdeath_BTInit();

  flamingdeath_GPSInit();
  //GPS_sent_init_messages();
  flamingdeath_AccelInit();
  flamingdeath_SensorTimerInit();
  // wait until things have settled
  delay_ms(1000);

  flamingdeath_ServoTimerInit();
}
